import math
from pickle import LONG1
import numpy

def CalJoint(point):

    def sqrt(a,b):
        return math.sqrt(a**2 + b**2)

    x,y,z = point

    L1 = 0.330
    L2 = 0.384
    L3 = 0.316
    LendFront = 0.088
    LendHeight = 0.107 + 0.17

    E3= 0.085
    E2 = 0.085

    L2F = sqrt(L2,E2)
    L3F = sqrt(L3,E3)

    dh =  z - L1 + LendHeight
    r = sqrt(x,y) - LendFront
    dist = sqrt(r,dh)



    def joint1(y,x):
        return math.degrees(math.atan(y/x))

    def L2A(a,b,c):
        
        cosA = (b ** 2 + c ** 2 - a ** 2) / (2 * b * c)
        cosB = (a ** 2 + c ** 2 - b ** 2) / (2 * a * c)
        cosC = (a ** 2 + b ** 2 - c ** 2) / (2 * a * b)

        angleA = math.degrees(math.acos(cosA))
        angleB = math.degrees(math.acos(cosB))
        angleC = math.degrees(math.acos(cosC))

        return angleA,angleB,angleC

    aL2F,aL3F,adist1 = L2A(L2F,L3F,dist)
    adh,ar,adist2 = L2A(dh,r,dist)
    aE3,aL3,alL3F = L2A(E3,L3,L3F)
    aE2,aL2,alL2F = L2A(E2,L2,L2F)

    j4 = 180 - (180 - (360 - aL2 - aL3 - adist1))
    j2 = 90 - adh - aL3F - aE2
    j7 = ar + aL2F + aE3
    
    j1 = joint1(y,x)
    print("target:",point)
    print("joint1:",j1)
    print("joint2:",j2)
    print("joint4:",j4)
    print("joint7:",j7)

    return j1,j2,j4,j7


# print(CalJoint([0.4, 0.4, 0.3]))